#include <pluginlib/class_loader.hpp>

// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit_msgs/msg/planning_scene.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <sensor_msgs/msg/joint_state.hpp>

using namespace rclcpp;

static const Logger LOGGER = get_logger("BLSmartRobot");

int main(int argc, char** argv)
{
  init(argc, argv);
  NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  std::shared_ptr<Node> robot_node =
      	Node::make_shared("BLSmartRobot", node_options);

  executors::SingleThreadedExecutor executor;
  executor.add_node(robot_node);
  std::thread([&executor]() { executor.spin(); }).detach();
  
  const std::shared_ptr<Publisher<std_msgs::msg::String>> publisher_ = 
  	robot_node->create_publisher<std_msgs::msg::String>("/BorunteArm/GoalJointState", 10);
  
  const std::shared_ptr<Publisher<sensor_msgs::msg::JointState>> jointStatePublisher_ = 
  	robot_node->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);
  
  const std::shared_ptr<Publisher<std_msgs::msg::String>> cmdLestistener = 
  	create_subscription<std_msgs::msg::String>( "BLSmartRobot/cmd", 10, std::bind(&BorunteRobot::run, this, _1));
  
  auto message = std_msgs::msg::String();
  message.data = "ready to publish! ";// + std::to_string(count_++);
  publisher_ -> publish(message);

  // BEGIN_TUTORIAL
  // Start
  // ^^^^^
  // Setting up to start using a planner is pretty easy. Planners are
  // setup as plugins in MoveIt and you can use the ROS pluginlib
  // interface to load any planner that you want to use. Before we can
  // load the planner, we need two objects, a RobotModel and a
  // PlanningScene. We will start by instantiating a
  // :moveit_codedir:`RobotModelLoader<moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h>`
  // object, which will look up the robot description on the ROS
  // parameter server and construct a
  // :moveit_codedir:`RobotModel<moveit_core/robot_model/include/moveit/robot_model/robot_model.h>`
  // for us to use.
  const std::string PLANNING_GROUP = "the_arm";
  robot_model_loader::RobotModelLoader robot_model_loader(robot_node, "robot_description");
  const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel();
  /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/
  moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model));
  const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

  // Using the
  // :moveit_codedir:`RobotModel<moveit_core/robot_model/include/moveit/robot_model/robot_model.h>`,
  // we can construct a
  // :moveit_codedir:`PlanningScene<moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h>`
  // that maintains the state of the world (including the robot).
  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

  // Configure a valid robot state
  planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");


// Added 
//print joint infos
  const std::vector<std::string>& joint_names = joint_model_group->getJointModelNames() ;
 // Get Joint Values
  std::vector<double> joint_valuesXX;
  RCLCPP_INFO(LOGGER, "Print joint values, start ...");
  planning_scene->getCurrentState().printStatePositions();
  planning_scene->getCurrentState().copyJointGroupPositions( joint_model_group, joint_valuesXX);
  
  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    RCLCPP_INFO(LOGGER, "Joint %s: %d", joint_names[i].c_str(), (int)(joint_valuesXX[i]*180/3.1416) );
  }
  RCLCPP_INFO(LOGGER, "Print joint values, end.");


  // We will now construct a loader to load a planner, by name.
  // Note that we are using the ROS pluginlib library here.
  std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
  planning_interface::PlannerManagerPtr planner_instance;
  std::string planner_plugin_name;

  // We will get the name of planning plugin we want to load
  // from the ROS parameter server, and then load the planner
  // making sure to catch all exceptions.
  if (!robot_node->get_parameter("planning_plugin", planner_plugin_name))
    RCLCPP_FATAL(LOGGER, "Could not find planner plugin name");
  try
  {
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
        "moveit_core", "planning_interface::PlannerManager"));
  }
  catch (pluginlib::PluginlibException& ex)
  {
    RCLCPP_FATAL(LOGGER, "Exception while creating planning plugin loader %s", ex.what());
  }
  try
  {
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
    if (!planner_instance->initialize(robot_model, robot_node,
                                      robot_node->get_namespace()))
      RCLCPP_FATAL(LOGGER, "Could not initialize planner instance");
    RCLCPP_INFO(LOGGER, "Using planning interface '%s'", planner_instance->getDescription().c_str());
  }
  catch (pluginlib::PluginlibException& ex)
  {
    const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;
    for (const auto& cls : classes)
      ss << cls << " ";
    RCLCPP_ERROR(LOGGER, "Exception while loading planner '%s': %s\nAvailable plugins: %s", planner_plugin_name.c_str(),
                 ex.what(), ss.str().c_str());
  }

  moveit::planning_interface::MoveGroupInterface move_group(robot_node, PLANNING_GROUP);

  // Visualization
  // ^^^^^^^^^^^^^
  // The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools(robot_node, "base_link",
                                                      "the_arm", move_group.getRobotModel());
  visual_tools.enableBatchPublishing();
  visual_tools.deleteAllMarkers();  // clear all old markers
  visual_tools.trigger();

  /* Remote control is an introspection tool that allows users to step through a high level script
     via buttons and keyboard shortcuts in RViz */
  visual_tools.loadRemoteControl();

  /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);

  int ready = 0;
while( true ) {
	if( ready==0 ){
		continue;
	}
	ready=1;
	
  /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */
  //visual_tools.trigger();

  /* We can also use visual_tools to wait for user input */
  //visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

  // Pose Goal
  // ^^^^^^^^^
  // We will now create a motion plan request for the arm of the BRTIXRZ0805A
  // specifying the desired pose of the end-effector as input.
  visual_tools.trigger();
  planning_interface::MotionPlanRequest req;
  planning_interface::MotionPlanResponse res;
  geometry_msgs::msg::PoseStamped pose;
  pose.header.frame_id = "base_link";
  pose.pose.position.x = -0.01;
  pose.pose.position.y = 0.05;
  pose.pose.position.z = 1.1;
  pose.pose.orientation.w = 1.0;

  // A tolerance of 0.01 m is specified in position
  // and 0.01 radians in orientation
  std::vector<double> tolerance_pose(3, 0.01);
  std::vector<double> tolerance_angle(3, 0.01);

  // We will create the request as a constraint using a helper function available
  // from the
  // :moveit_codedir:`kinematic_constraints<moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h>`
  // package.
  moveit_msgs::msg::Constraints pose_goal =
      kinematic_constraints::constructGoalConstraints("link6", pose, tolerance_pose, tolerance_angle);

  req.group_name = PLANNING_GROUP;
  req.goal_constraints.push_back(pose_goal);

  // We now construct a planning context that encapsulate the scene,
  // the request and the response. We call the planner using this
  // planning context
  planning_interface::PlanningContextPtr context =
      planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);
  if (res.error_code_.val != res.error_code_.SUCCESS)
  {
    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
    return 0;
  }

  // Visualize the result
  // ^^^^^^^^^^^^^^^^^^^^
  std::shared_ptr<Publisher<moveit_msgs::msg::DisplayTrajectory>> display_publisher =
      robot_node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path",
                                                                                               1);
  moveit_msgs::msg::DisplayTrajectory display_trajectory;

  /* Visualize the trajectory */
  moveit_msgs::msg::MotionPlanResponse response;
  res.getMessage(response);
  
  std::vector<double> joint_valuesYY;
  res.trajectory_->getLastWayPoint().copyJointGroupPositions( joint_model_group, joint_valuesYY);

  display_trajectory.trajectory_start = response.trajectory_start;
  display_trajectory.trajectory.push_back(response.trajectory);
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher->publish(display_trajectory);

  /* Set the state in the planning scene to the final state of the last plan */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());

  // Display the goal state
  visual_tools.publishAxisLabeled(pose.pose, "goal_1");
  visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();
  
  RCLCPP_INFO(LOGGER, "Print current joint values, [1]");
  //planning_scene->getCurrentState().printStatePositions();
  robot_state->copyJointGroupPositions( joint_model_group, joint_valuesXX);
  
  auto message2 = std_msgs::msg::String();
  //char[] msgData = 
  message2.data="data:[";
  
  sensor_msgs::msg::JointState endJointState;
  	int nJointCount =  joint_names.size();
  	Clock theClock;
  	endJointState.header.stamp = theClock.now(); //ros::Time::now();
  	endJointState.name.resize(nJointCount) ; //= "base_link";
  	endJointState.position.resize(nJointCount);//= 
  	
  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    RCLCPP_INFO(LOGGER, "Joint %s: %d / %f", joint_names[i].c_str(), (int)(1000*joint_valuesXX[i]*180/3.1416) , joint_valuesXX[i] );
    if( i>0 ){
    	message2.data +=",";
    }
  	message2.data += joint_names[i] + ":"+ std::to_string( (int)(1000*joint_valuesXX[i]*180/3.1416) ) ;
  	
  	endJointState.name[i] = joint_names[i];
  	endJointState.position[i] = joint_valuesXX[i];
  }
  message2.data+="]";
  
  	publisher_ -> publish(message2);
  	
  	
  	jointStatePublisher_ -> publish(endJointState);
  
  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    RCLCPP_INFO(LOGGER, "Joint %s: %d", joint_names[i].c_str(), (int)(joint_valuesYY[i]*180/3.1416) );
  }
  
  RCLCPP_INFO(LOGGER, "Print Current joint values, [1]");
  
}

  shutdown();
  return 0;
}
